#include <MegabriteController.h>
#include <LinearColorMap.h>

MegabriteController* megabriteController;
LinearColorMap* colorMap;

void setup() {
  Serial.begin(9600);
  megabriteController = newMegabriteController();
  megabriteController->init();
  megabriteController->setNumberOfMegabrites(1);
  colorMap = newLinearColorMap();
  createLinearColorMap();
  Serial.println("initialized ");
}

void loop() {
  Serial.println("looping");
  for (int i = 0; i < 1000; i++) {
    sendColorValue(i);
  }
  for (int i = 1000; i >= 0; i--) {
    sendColorValue(i);
  }
}

void setMegabriteColor(int megabriteIdx, Color color) {
    Megabrite* megabrite = megabriteController->getMegabriteAtIndex(megabriteIdx);
    Serial.print(megabrite->red);
    Serial.print(" ");
    Serial.print(color.red);
    Serial.print(" ");
    Serial.print(color.green);
    Serial.print(" ");
    Serial.print(color.blue);
    Serial.println(" ");
    megabrite->red = color.red;
    megabrite->green = color.green;
    megabrite->blue = color.blue;
}

void sendColorValue(int i) {
    Color color = colorMap->getColorForValue(i);
    setMegabriteColor(0, color);
    megabriteController->send();
    delay(20);
}

void createLinearColorMap() {
  	colorMap = newLinearColorMap();
	colorMap->addRedPoint(0, 1023);
	colorMap->addRedPoint(200, 0);
	colorMap->addRedPoint(400, 0);
	colorMap->addRedPoint(600, 1023);
	colorMap->addRedPoint(800, 0);
	colorMap->addRedPoint(1000, 1023);

	colorMap->addGreenPoint(00, 0);
	colorMap->addGreenPoint(200, 0);
	colorMap->addGreenPoint(400, 800);
	colorMap->addGreenPoint(600, 0);
	colorMap->addGreenPoint(800, 600);
	colorMap->addGreenPoint(1000, 1023);

	colorMap->addBluePoint(0, 0);
	colorMap->addBluePoint(200, 1023);
	colorMap->addBluePoint(400, 0);
	colorMap->addBluePoint(600, 600);
	colorMap->addBluePoint(800, 1023);
	colorMap->addBluePoint(1000, 1023);
}
